// Davis Logic V2: DULLEA Ecosystem
// File: dullea_tmr_voter.hpp
#ifndef DULLEA_TMR_VOTER_HPP
#define DULLEA_TMR_VOTER_HPP

#include "dullea_core_types.hpp"

namespace Dullea {
    class TmrSensorVoter {
    public:
        enum class VoterStatus { NOMINAL_ALL_VALID, DEGRADED_ONE_REJECTED, CRITICAL_FAULT };

        /**
         * @brief Constructor
         * @param max_allowed_deviation Maximum allowable discrepancy between sensors in q15_16_t before rejection.
         */
        inline TmrSensorVoter(q15_16_t max_allowed_deviation) 
            : m_max_deviation(max_allowed_deviation), m_status(VoterStatus::NOMINAL_ALL_VALID) {}

        /**
         * @brief Processes three independent channels and returns a voted, safe value.
         * @param s1 Sensor Channel 1 value (q15_16_t)
         * @param s2 Sensor Channel 2 value (q15_16_t)
         * @param s3 Sensor Channel 3 value (q15_16_t)
         * @param out_voted_val Reference to store the validated output value.
         * @return Current structural operational status of the sensor triad.
         */
        inline VoterStatus vote(q15_16_t s1, q15_16_t s2, q15_16_t s3, q15_16_t& out_voted_val) {
            // Compute absolute delta differences using manual absolute paths to avoid std library links
            q15_16_t diff_1_2 = (s1 > s2) ? (s1 - s2) : (s2 - s1);
            q15_16_t diff_2_3 = (s2 > s3) ? (s2 - s3) : (s3 - s2);
            q15_16_t diff_3_1 = (s3 > s1) ? (s3 - s1) : (s1 - s3);

            bool match_1_2 = (diff_1_2 <= m_max_deviation);
            bool match_2_3 = (diff_2_3 <= m_max_deviation);
            bool match_3_1 = (diff_3_1 <= m_max_deviation);

            // Case 1: All sensors agree within safe threshold bounds
            if (match_1_2 && match_2_3 && match_3_1) {
                m_status = VoterStatus::NOMINAL_ALL_VALID;
                // Return mid-point median via conditional sorts
                if ((s1 >= s2 && s1 <= s3) || (s1 <= s2 && s1 >= s3)) out_voted_val = s1;
                else if ((s2 >= s1 && s2 <= s3) || (s2 <= s1 && s2 >= s3)) out_voted_val = s2;
                else out_voted_val = s3;
                return m_status;
            }

            // Case 2: Sensor 3 has faulted or drifted out of bounds
            if (match_1_2) {
                m_status = VoterStatus::DEGRADED_ONE_REJECTED;
                out_voted_val = (s1 + s2) >> 1; // Average the two surviving valid channels
                return m_status;
            }

            // Case 3: Sensor 1 has faulted or drifted out of bounds
            if (match_2_3) {
                m_status = VoterStatus::DEGRADED_ONE_REJECTED;
                out_voted_val = (s2 + s3) >> 1;
                return m_status;
            }

            // Case 4: Sensor 2 has faulted or drifted out of bounds
            if (match_3_1) {
                m_status = VoterStatus::DEGRADED_ONE_REJECTED;
                out_voted_val = (s1 + s3) >> 1;
                return m_status;
            }

            // Case 5: Multiple sensor failure or triad disagreement. Catastrophic condition.
            m_status = VoterStatus::CRITICAL_FAULT;
            out_voted_val = 0; // Force zero output state to trigger system isolation containment
            return m_status;
        }

        inline VoterStatus get_status() const { return m_status; }

    private:
        q15_16_t m_max_deviation;
        VoterStatus m_status;
    };
}
#endif
